'' Octopilot -- Stabilization.spin
'' (c) 2008-2009 Ben Levitt
'' http://code.google.com/p/octopilot/
''
'' Adjust the ailerons and elevator based on IMU data (gyroscopes and accelerometers).
'' Take into account the current navigation state, so we can help with turning.


OBJ
  IMU       :  "IMU-andRange"
  fMath     :  "FloatMath"
  rollAxis  :  "PIDController"        
  pitchAxis :  "PIDController"        
  config    :  "Config"
  ' debug     : "FullDuplexSerial"


CON
  pitchTargetOffset             =    20                 ' 10 = pitches the plane 1 degree up
  rollTargetOffset              =   -10                 ' 10 = rolls the plane 1 degree right

  
VAR
  long inValsAddr
  long outValsAddr
  long defaultValsAddr
  long navStateAddr
  long range
  
  long pitch, dPitch, tPitch
  long roll, dRoll, tRoll
  long deflection
  
  ' word throttleMultiplier
  byte navState 

  
PUB Start(insAddr, outsAddr, defaultsAddr, _navStateAddr)
  inValsAddr := insAddr      
  outValsAddr := outsAddr
  defaultValsAddr := defaultsAddr
  navStateAddr := _navStateAddr
  IMU.start(config#imuPinBase, config#imuPinBase+1, config#imuPinBase+2, 1)
  rollAxis.Start(8.0, 20.0, 1.2, -4000, 4000)
  pitchAxis.Start(8.0, 20.0, 1.2, -4000, 4000)
  ' debug.Start(config#debugPinBase+1, config#debugPinBase, 0, 38400)


PUB EnabledAutopilot
  rollAxis.Reset
  pitchAxis.Reset


PUB Apply
  ' Get current IMU values
  pitch := fMath.FRound(fMath.FMul(IMU.get_angle_Pitch, fMath.FFloat(10)))
  dPitch:= fMath.FRound(fMath.FMul(IMU.get_rate_Pitch,  fMath.FFloat(10)))
  tPitch:= pitchTargetOffset    ' Adjust for IMU not being exactly coplanar with level flight
  
  roll  := fMath.FRound(fMath.FMul(IMU.get_angle_Roll,  fMath.FFloat(10)))
  dRoll := fMath.FRound(fMath.FMul(IMU.get_rate_Roll,   fMath.FFloat(10)))
  tRoll := rollTargetOffset     ' Adjust for IMU not being exactly coplanar with level flight
  
  
  if config#stabilizationProximityEnabled
    ' Get and make use of range value
    range := IMU.get_range
  
  
  ' Get NavState, and use it to adjust the target roll angle (lean into turns)
  navState := BYTE[navStateAddr]
  if (navState & config#NavTurningLeft) or (WORD[inValsAddr+config#aileron*2] < 13500)
    tRoll += -60                                        ' 2 degrees
    tPitch+=  20                                        ' 2 degrees
  elseif (navState & config#NavTurningRight) or (WORD[inValsAddr+config#aileron*2] > 16500)
    tRoll +=  60                                        ' 6 degrees
    tPitch+=  20                                        ' 2 degrees
  
   ' If transmitter is driving elevator full up or down, then angle up or down
  if WORD[inValsAddr+config#elevator*2] < 13500
    tPitch +=  50
  elseif WORD[inValsAddr+config#elevator*2] > 16500
    tPitch += -50
  
  
  ' throttleMultiplier := (30_000 - WORD[inValsAddr+config#throttle*2]) / 200     ' (50 to 100)
  
  'debug.dec(pitch)
  'debug.tx(",")
  'debug.tx(" ")
  'debug.dec(dPitch)
  'debug.tx(",")
  'debug.tx(" ")
  'debug.dec(tPitch)
  'debug.tx(",")
  'debug.tx(" ")
  
  deflection := pitchAxis.GetOutput(pitch, dPitch, tPitch)
  config.setElevator(outValsAddr, defaultValsAddr, deflection)
  
  'debug.dec(deflection)
  'debug.tx(13)
  
  deflection := rollAxis.GetOutput(roll, dRoll, tRoll)
  config.setAileron(outValsAddr, defaultValsAddr, deflection)
 